#ifndef SERVOSETTING_H
#define SERVOSETTING_H

#include <QObject>
#include "ecat_slave_define.h"
#include "ecat_manager.h"
#include "ecat_cmd_define.h"
#include <QSet>
#include <QDebug>
#include <QReadWriteLock>
#include <QReadLocker>
#include <QWriteLocker>

#define SERVO ServoSetting::get_instance()

class ServoSetter; // forward declare

/**
 * @brief The ServoSetting class
 * 提供对驱动器的使能和清错控制设置
 * 包括：
 * 使能选定的驱动器
 * 选择是否自动清错
 * 根据以上设置，查询EcatManager中的驱动器状态，并设置对应的控制字，但不进行同步
 */
class ServoSetting : public QObject
{
    Q_OBJECT
    friend class ServoSetter;
private:
    explicit ServoSetting(QObject *parent = nullptr);
    ServoSetting(const ServoSetting&) = delete ;
    ServoSetting& operator=(const ServoSetting&) = delete ;

public:
    enum ServoRunningState {
        EnablingState = 0x1,
        CommonState = 0x2,
        DisablingState = 0x4,
        ClearingFaultState = 0x8,
        StartingState = EnablingState | ClearingFaultState,
        StopingState = DisablingState
    };

    static ServoSetting* get_instance();

    void set_servo_gear_ratio(int slave_num, double gear_ratio);
    void set_servo_gear_ratio(std::array<double, SERVO_NUM> gear_ratio_arr);

    void set_servo_enable(int slave_num, bool enable = true);
    void set_servo_disable(int slave_num, bool disable = true) {set_servo_enable(slave_num, !disable);}
    bool is_servo_enable(int slave_num) const; // 指用户是否设置了该驱动器使能，不是驱动器的真实状态机中的使能
    void set_servo_auto_clear_fault(bool auto_clear) {auto_clear_fault_ = auto_clear;}
    bool is_servo_auto_clear_fault() const {return auto_clear_fault_;}

    /**
     * @brief 根据设置的使能驱动器集合、是否清错，进行一次ecat控制设置
     * 会根据状态设置控制字，但不会调用EcatManager::sync()
     */
    void commit_setting_once();

    // 获取各个坐标轴位置
    double get_axis_actual_value(uint32_t axis) const;

    // 设置各个坐标轴的目标位置
    void set_axis_target_value(uint32_t axis, double value);

private:
    void clear_fault_once_();
    void set_enable_once_();

    void update_axis_actual_value_();
    void set_to_ecat_axis_target_value_();

private:
    QVector<bool> qvec_servo_enabled_; // 驱动器是否使能，数组维度为SLAVE_NUM + 1，下标从1开始取
    bool auto_clear_fault_{false}; // 是否自动清错，默认否
    std::array<double, SERVO_NUM + 1> gear_ratio_; // 减速比

private:
    mutable QReadWriteLock rd_wr_lock_;

private:
    std::array<double, SERVO_NUM + 1> act_axis_value_;
    std::array<double, SERVO_NUM + 1> req_axis_value_;

signals:

};

class ServoSetter : public QObject
{
    Q_OBJECT
public:
    ServoSetter(ServoSetting* ss = SERVO->get_instance()) : _ss(ss) {
        Q_ASSERT(_ss != nullptr);
        // 执行清错、使能（取消使能）
        _ss->commit_setting_once();

        // 从ECAT获取当前位置，并转化为坐标暂存于ServoSetting::act_axis_value_中
        _ss->update_axis_actual_value_();
        _ss->req_axis_value_ = _ss->act_axis_value_;
    }
    ~ServoSetter() {
        Q_ASSERT(_ss != nullptr);
        if (ECAT->servo_state_has_fault()) {
            // 如果有错就不发送位置
        } else {
            // 如果驱动器已经使能，且ServoSetting中设置为使能
            // 这么判断是为了防止commit_setting_once取消使能和清错的命令被覆盖
            // 根据ServoSetting::req_axis_value_中的设置数据更新ecat的目标位置
            for (int i = 1; i <= SERVO_NUM; ++i) {
                if (ECAT->servo_state_operation_enabled(i) && _ss->is_servo_enable(i)) {
                    ECAT->set_servo_control_word(0, CW_EnableOperation);
                    ECAT->set_servo_operation_mode(0, OM_CSP);
                }
            }
            _ss->set_to_ecat_axis_target_value_();
        }

    }
private:
    ServoSetting *_ss;
    Q_DISABLE_COPY(ServoSetter);
    void* operator new(size_t) = delete;
};

#endif // ServoSetting_H
